#include "Servo_Control.h"
#include "tim.h"
#include "bsp_uart.h"
#include "math.h"
#include <stdlib.h>

float servo_current_angle[4]={0,0,0,0};
float servo_expected_angle[4]={0,0,0,0};
float servo_velocity[4]={1,1,1,1};
static float seamax[4]={180	,270,270,120};
static float seamin[4]={90	,0	,0	,60	};

SVT svqueue[SV_QUEUE_LENGTH];
uint8_t svthead,svtrear;

uint8_t arm_is_moving=0;

void Get_Servo_Control_Msg(void)
{
    if (RC_DATA.rc_s1 == 1)
    {
        arm_motion(0);
    }
    if (RC_DATA.rc_s1 == 3) //遥控器控制逻辑
    {
        if (RC_DATA.rc_ch1 > 300)
            arm_motion(1);
        else if (RC_DATA.rc_ch1 < -300)
            arm_motion(2);
        else
            arm_motion(0);
        if (RC_DATA.rc_s2==2)
            arm_motion(10);
        else 
            arm_motion(11);
    }

}

uint32_t angle_conv(uint8_t servo_num,float angle){
	uint32_t dutycycle,max_angle;
	if(servo_num==0){
		max_angle=180;
		if(angle>max_angle) angle=max_angle;
		else if(angle<0) angle=0;
		dutycycle=(uint32_t)(1911*angle/max_angle+610);
	}
	else if(servo_num==1){
		max_angle=270;
		if(angle>max_angle) angle=max_angle;
		else if(angle<0) angle=0;
		dutycycle=(uint32_t)(2000*angle/max_angle+499);
	}
	else if(servo_num==2){
		max_angle=270;
		if(angle>max_angle) angle=max_angle;
		else if(angle<0) angle=0;
		dutycycle=(uint32_t)(2000*angle/max_angle+499);
	}
	else if(servo_num==3){
		max_angle=180;
		if(angle>max_angle) angle=max_angle;
		else if(angle<0) angle=0;
		dutycycle=(uint32_t)(2000*angle/max_angle+499);
	}
	else dutycycle=0;
	return dutycycle;
}

void servo_apply(uint8_t servo_num,float angle){
	if(servo_num==0){
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, angle_conv(0,angle));
		servo_current_angle[0]=angle;
	}
	else if(servo_num==1){
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, angle_conv(1,angle));
		servo_current_angle[2]=angle;
	}
	else if(servo_num==2){
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, angle_conv(2,angle));
		servo_current_angle[2]=angle;
	}
	else if(servo_num==3){
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, angle_conv(3,angle));
		servo_current_angle[3]=angle;
	}
	else return;
}

void servo_set(uint8_t servo_num,float angle,float velocity){
	if(angle>seamax[servo_num]) servo_expected_angle[servo_num]=seamax[servo_num];
	else if(angle<seamin[servo_num]) servo_expected_angle[servo_num]=seamin[servo_num];
	else servo_expected_angle[servo_num]=angle;
	if(velocity>1) servo_velocity[servo_num]=1;
	else if(velocity<0.1f) servo_velocity[servo_num]=0.1;
	else servo_velocity[servo_num]=velocity;
}

void servo_edit_next_task(uint8_t servo_num,float angle,float velocity){
	uint8_t task_num;
	if(svtrear>=SV_QUEUE_LENGTH-1) task_num=0;
	else task_num=svtrear+1;
	svqueue[task_num].s_angle[servo_num]=angle;
	svqueue[task_num].s_vel[servo_num]=velocity;
	svqueue[task_num].keep_prev[servo_num]=0;
}

void servo_confirm_next_task(void){
	if(svtrear>=SV_QUEUE_LENGTH-1) svtrear=0;
	else svtrear++;
}

void servo_init(float s0_angle,float s1_angle,float s2_angle,float s3_angle){
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4);
	
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, angle_conv(0,s0_angle));
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, angle_conv(1,s1_angle));
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, angle_conv(2,s2_angle));
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, angle_conv(3,s3_angle));
	
	servo_current_angle[0]=s0_angle;
	servo_current_angle[1]=s1_angle;
	servo_current_angle[2]=s2_angle;
	servo_current_angle[3]=s3_angle;
	servo_expected_angle[0]=s0_angle;
	servo_expected_angle[1]=s1_angle;
	servo_expected_angle[2]=s2_angle;
	servo_expected_angle[3]=s3_angle;
	
	svthead=0;
	svtrear=0;
	for(uint8_t i=0;i<SV_QUEUE_LENGTH;i++)
		for(uint8_t j=0;j<4;j++)
			svqueue[i].keep_prev[j]=1;
//	servo_edit_next_task(0,s0_angle,1);
//	servo_edit_next_task(1,s1_angle,1);
//	servo_edit_next_task(2,s2_angle,1);
//	servo_edit_next_task(3,s3_angle,1);
//	servo_confirm_next_task();
}

uint8_t arm_motion(uint8_t type){
//	if(arm_is_moving) 
//		return 1;
//	else {
		if(type==0){//arm hold
			servo_edit_next_task(0,180,0.6);
			servo_edit_next_task(1,0,1);
			servo_edit_next_task(2,80,1);
			servo_confirm_next_task();
		}
		else if(type==1){//arm grab
			servo_edit_next_task(0,150,0.6);
			servo_edit_next_task(1,130,1);
			servo_edit_next_task(2,45,1);
			servo_confirm_next_task();
		}
		else if(type==2){//arm drop
			servo_edit_next_task(0,140,0.6);
			servo_edit_next_task(1,90,0.7);
			servo_edit_next_task(2,250,1);
			servo_confirm_next_task();
		}
		else if(type==10){//hand grab
			servo_edit_next_task(3,105,1);
			servo_confirm_next_task();
		}
		else if(type==11){//hand drop
			servo_edit_next_task(3,80,1);
			servo_confirm_next_task();
		}
		else if(type==20){//grab and drop and hold
			servo_edit_next_task(3,105,1);
			servo_confirm_next_task();
			servo_edit_next_task(0,130,0.6);
			servo_edit_next_task(1,65,0.4);
			servo_edit_next_task(2,210,1);
			servo_confirm_next_task();
			servo_edit_next_task(3,80,1);
			servo_confirm_next_task();
			servo_edit_next_task(0,180,0.6);
			servo_edit_next_task(1,0,1);
			servo_edit_next_task(2,90,1);
			servo_edit_next_task(3,105,1);
			servo_confirm_next_task();
		}
		else if(type==21){//prepare to grab
			servo_edit_next_task(0,150,0.6);
			servo_edit_next_task(1,130,1);
			servo_edit_next_task(2,45,1);
			servo_edit_next_task(3,80,1);
			servo_confirm_next_task();
		}
//		
//	}
	return 0;
}
